//
// PicoMax
// Copyright 2012 Thomas W. Holtquist
// www.skewworks.com
//
// GPL 3.0 Licensed
//

using System;
using System.IO;

using Microsoft.SPOT;

namespace Skewworks.PicoMax.Graphics565
{

    /// <summary>
    /// Minimal bitmap support for 565 screens
    /// </summary>
    public class PicoBitmap
    {

        #region Variables

        private int _w;
        private int _h;
        private byte[] _data;
        private bool _useOffset;
        private int _iStart = 0;
        private Rect _clipRegion;

        #endregion

        #region Constructors

        /// <summary>
        /// Creates a new PicoBitmap
        /// </summary>
        /// <param name="Width">Width of image</param>
        /// <param name="Height">Height of image</param>
        public PicoBitmap(int Width, int Height)
        {
            _data = new byte[Width * Height * 2];
            _w = Width;
            _h = Height;
            _clipRegion = new Rect(0, 0, _w, _h);
        }

        /// <summary>
        /// Creates a PicoBitmap from file
        /// </summary>
        /// <param name="Filename">Path to PicoBitmap</param>
        public PicoBitmap(string Filename)
        {
            byte[] PicoBmpData = File.ReadAllBytes(Filename);
            _w = (PicoBmpData[0] << 8) + PicoBmpData[1];
            _h = (PicoBmpData[2] << 8) + PicoBmpData[3];
            _data = new byte[PicoBmpData.Length - 4];
            Array.Copy(PicoBmpData, 4, _data, 0, _data.Length);
            PicoBmpData = null;
            _clipRegion = new Rect(0, 0, _w, _h);
        }

        /// <summary>
        /// Creates a PicoBitmap from raw data
        /// </summary>
        /// <param name="Pixels">Array of pixel data</param>
        /// <param name="Width">Width of image</param>
        /// <param name="Height">Height of image</param>
        public PicoBitmap(byte[] Pixels, int Width, int Height)
        {
            if (Pixels.Length != Width * Height * 2)
                throw new Exception("Invalid byte length for supplied dimensions");

            _data = Pixels;
            _w = Width;
            _h = Height;
            _clipRegion = new Rect(0, 0, _w, _h);
        }

        /// <summary>
        /// Creates a PicoBitmap from raw data
        /// </summary>
        /// <param name="PicoBmpData">PicoBitmap data</param>
        public PicoBitmap(byte[] PicoBmpData, bool RAMSaverMode = false)
        {
            _w = (PicoBmpData[0] << 8) + PicoBmpData[1];
            _h = (PicoBmpData[2] << 8) + PicoBmpData[3];

            if (RAMSaverMode)
            {
                _data = PicoBmpData;
                _useOffset = true;
                _iStart = 4;
            }
            else
            {
                _data = new byte[PicoBmpData.Length - 4];
                Array.Copy(PicoBmpData, 4, _data, 0, _data.Length);
            }

            _clipRegion = new Rect(0, 0, _w, _h);
        }

        #endregion

        #region Properties

        /// <summary>
        /// Gets/Sets the clipping region for drawing on the PicoBitmap
        /// </summary>
        public Rect ClippingRegion
        {
            get { return _clipRegion; }
            set
            {
                if (value.X < 0 || value.X > _w || value.Y < 0 || value.Y > _h)
                    value = new Rect(0, 0, 0, 0);
                if (value.X + value.Width > _w)
                    value.Width = _w - value.X;
                if (value.Y + value.Height > _h)
                    value.Height = _h - value.Y;
                _clipRegion = value;
            }
        }

        /// <summary>
        /// Gets height of image
        /// </summary>
        public int Height
        {
            get { return _h; }
        }

        /// <summary>
        /// Gets/Sets Pixel data
        /// WARNING: Do not alter size of array
        /// </summary>
        public byte[] Pixels
        {
            get { return _data; }
            set { _data = value; }
        }

        /// <summary>
        /// True when RAM saving mode is enabled
        /// </summary>
        public bool RAMSavingMode
        {
            get { return _useOffset; }
        }

        /// <summary>
        /// Gets width of image
        /// </summary>
        public int Width
        {
            get { return _w; }
        }

        #endregion

        #region Public Methods (General)

        /// <summary>
        /// Clears image (to black)
        /// </summary>
        public void Clear()
        {
            for (int i = _iStart; i < _data.Length; i++)
                _data[i] = 0;
        }

        /// <summary>
        /// Clears image to specificed color
        /// </summary>
        /// <param name="Color"></param>
        public void Clear(RGB565 Color)
        {
            Clear(Color.Value);
        }

        /// <summary>
        /// Clears image to specified color
        /// </summary>
        /// <param name="Color"></param>
        public void Clear(ushort Color)
        {
            byte c1 = (byte)(Color >> 8);
            byte c2 = (byte)Color;

            for (int i = 0; i < _data.Length; i += 2)
            {
                _data[i] = c1;
                _data[i + 1] = c2;
            }
        }

        /// <summary>
        /// Draws an ellipse
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="XRadius"></param>
        /// <param name="YRadius"></param>
        public void DrawEllipse(RGB565 Color, int X, int Y, int XRadius, int YRadius)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0)
                return;

            DrawEllipse(Color.Value, X, Y, XRadius, YRadius);
        }

        public void DrawEllipse(ushort Color, int X, int Y, int XRadius, int YRadius)
        {
            int Rx2 = XRadius * XRadius;
            int Ry2 = YRadius * YRadius;
            int twoRx2 = 2 * Rx2;
            int twoRy2 = 2 * Ry2;
            int p;
            int x = 0;
            int y = YRadius;
            int px = 0;
            int py = twoRx2 * y;

            ellipsePlotPoints(X, Y, x, y, Color);

            // Region 1
            p = Round(Ry2 - (Rx2 * YRadius) + 0.25 * Rx2);

            while (px < py)
            {
                x++;
                px += twoRy2;
                if (p < 0)
                    p += Ry2 + px;
                else
                {
                    y--;
                    py -= twoRx2;
                    p += Ry2 + px - py;
                }

                ellipsePlotPoints(X, Y, x, y, Color);
            }

            // Region 2
            p = Round(Ry2 * (x + 0.5) * (x + 0.5) + Rx2 * (y - 1) * (y - 1) - Rx2 * Ry2);

            while (y > 0)
            {
                y--;
                py -= twoRx2;
                if (p > 0)
                    p += Rx2 - py;
                else
                {
                    x++;
                    px += twoRy2;
                    p += Rx2 - py + px;
                }

                ellipsePlotPoints(X, Y, x, y, Color);
            }

        }

        /// <summary>
        /// Draws an ellipse
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="XRadius"></param>
        /// <param name="YRadius"></param>
        /// <param name="FillColor"></param>
        public void DrawEllipse(RGB565 Color, int X, int Y, int XRadius, int YRadius, RGB565 FillColor)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0)
                return;

            DrawEllipse(Color.Value, X, Y, XRadius, YRadius, FillColor.Value);
        }

        public void DrawEllipse(ushort Color, int X, int Y, int XRadius, int YRadius, ushort FillColor)
        {
            int Rx2 = XRadius * XRadius;
            int Ry2 = YRadius * YRadius;
            int twoRx2 = 2 * Rx2;
            int twoRy2 = 2 * Ry2;
            int p;
            int x = 0;
            int y = YRadius;
            int px = 0;
            int py = twoRx2 * y;

            ellipsePlotPoints(X, Y, x, y, Color);

            // Region 1
            p = Round(Ry2 - (Rx2 * YRadius) + 0.25 * Rx2);

            bool bStart = false;
            bool bDrawn = false;

            while (px < py)
            {
                x++;
                px += twoRy2;
                if (p < 0)
                    p += Ry2 + px;
                else
                {
                    y--;
                    py -= twoRx2;
                    p += Ry2 + px - py;
                    bStart = true;
                    bDrawn = false;
                }

                if (bStart && !bDrawn)
                {
                    FillRectangle(FillColor, X - x, Y - y, (X + x) - (X - x), 1);
                    FillRectangle(FillColor, X - x, Y + y, (X + x) - (X - x), 1);

                    bDrawn = true;
                }

                ellipsePlotPoints(X, Y, x, y, Color);
            }

            FillRectangle(FillColor, X - x, Y - y + 1, (X + x) - (X - x) + 1, (Y + y) - (Y - y) - 1);

            // Region 2
            p = Round(Ry2 * (x + 0.5) * (x + 0.5) + Rx2 * (y - 1) * (y - 1) - Rx2 * Ry2);

            bDrawn = false;
            while (y > 0)
            {
                y--;
                py -= twoRx2;
                if (p > 0)
                    p += Rx2 - py;
                else
                {
                    x++;
                    px += twoRy2;
                    p += Rx2 - py + px;
                    bDrawn = false;
                }

                if (!bDrawn)
                {
                    FillRectangle(FillColor, X - x, Y - y, 1, (Y + y) - (Y - y));
                    FillRectangle(FillColor, X + x, Y - y, 1, (Y + y) - (Y - y));

                    bDrawn = true;
                }

                ellipsePlotPoints(X, Y, x, y, Color);
            }
        }

        public void DrawEllipse(RGB565 OutlineColor, int X, int Y, int XRadius, int YRadius, RGB565 Gradient1, RGB565 Gradient2)
        {
            int Rx2 = XRadius * XRadius;
            int Ry2 = YRadius * YRadius;
            int twoRx2 = 2 * Rx2;
            int twoRy2 = 2 * Ry2;
            int p;
            int x = 0;
            int y = YRadius;
            int px = 0;
            int py = twoRx2 * y;

            // Get Values
            double R = Gradient1.Red;
            double G = Gradient1.Green;
            double B = Gradient1.Blue;

            double Rb = Gradient2.Red;
            double Gb = Gradient2.Green;
            double Bb = Gradient2.Blue;

            double rS = (double)(Gradient2.Red - Gradient1.Red) / (YRadius * 2);
            double gS = (double)(Gradient2.Green - Gradient1.Green) / (YRadius * 2);
            double bS = (double)(Gradient2.Blue - Gradient1.Blue) / (YRadius * 2);
            RGB565 c;

            ellipsePlotPoints(X, Y, x, y, OutlineColor.Value);

            // Region 1
            p = Round(Ry2 - (Rx2 * YRadius) + 0.25 * Rx2);

            bool bStart = false;
            bool bDrawn = false;

            while (px < py)
            {
                x++;
                px += twoRy2;
                if (p < 0)
                    p += Ry2 + px;
                else
                {
                    y--;
                    py -= twoRx2;
                    p += Ry2 + px - py;
                    bStart = true;
                    bDrawn = false;

                    R += rS;
                    B += bS;
                    G += gS;

                    Rb -= rS;
                    Bb -= bS;
                    Gb -= gS;
                }

                if (bStart && !bDrawn)
                {
                    c = new RGB565((byte)R, (byte)G, (byte)B);
                    FillRectangle(c.Value, X - x, Y - y, (X + x) - (X - x), 1);


                    c = new RGB565((byte)Rb, (byte)Gb, (byte)Bb);
                    FillRectangle(c.Value, X - x, Y + y, (X + x) - (X - x), 1);

                    bDrawn = true;
                }

                ellipsePlotPoints(X, Y, x, y, OutlineColor.Value);
            }

            //FillRectangle(FillColor, X - x, Y - y + 1, (X + x) - (X - x) + 1, (Y + y) - (Y - y) - 1);

            // Region 2
            p = Round(Ry2 * (x + 0.5) * (x + 0.5) + Rx2 * (y - 1) * (y - 1) - Rx2 * Ry2);

            bDrawn = false;
            while (y > 0)
            {
                y--;
                py -= twoRx2;
                if (p > 0)
                    p += Rx2 - py;
                else
                {
                    x++;
                    px += twoRy2;
                    p += Rx2 - py + px;
                }

                //FillRectangle(Colors565.Yellow, X - x, Y - y + 1, (X + x) - (X - x) + 1, (Y + y) - (Y - y) - 1);
                //FillRectangle(Colors565.Yellow, X - x, Y - y, 1, (Y + y) - (Y - y));


                c = new RGB565((byte)R, (byte)G, (byte)B);
                FillRectangle(c.Value, X - x, Y - y, (X + x) - (X - x) + 1, 1);
                c = new RGB565((byte)Rb, (byte)Gb, (byte)Bb);
                FillRectangle(c.Value, X - x, Y + y, (X + x) - (X - x) + 1, 1);

                ellipsePlotPoints(X, Y, x, y, OutlineColor.Value);

                R += rS;
                B += bS;
                G += gS;

                Rb -= rS;
                Bb -= bS;
                Gb -= gS;
            }
        }

        /// <summary>
        /// Draws another image on top of the current one at the specified location
        /// </summary>
        /// <param name="Bitmap">PicoBitmap to draw</param>
        /// <param name="X">X to draw at</param>
        /// <param name="Y">Y to draw at</param>
        public void DrawImage(PicoBitmap Bitmap, int X, int Y)
        {
            int BytesPerLine = Bitmap.Width * 2;
            int LineCount = Bitmap.Height;
            int xOffset = 0;
            int LineStart = 0;

            // Check bounds
            if (_clipRegion.Width == 0 || _clipRegion.Height == 0 || X >= _clipRegion.X + _clipRegion.Width || Y >= _clipRegion.Y + _clipRegion.Height)
                return;

            // Adjust X
            if (X < 0)
            {
                xOffset = -X * 2;
                X = 0;
            }
            if (X < _clipRegion.X)
            {
                xOffset += (_clipRegion.X - X) * 2;
                X = _clipRegion.X;
            }
            
            // Adjust Y
            if (Y < 0)
            {
                LineStart = -Y;
                Y = 0;
            }
            if (Y < _clipRegion.Y)
            {
                LineStart += _clipRegion.Y - Y;
                Y = _clipRegion.Y;
            }

            // Adjust Width
            if (X + Bitmap.Width > _clipRegion.X + _clipRegion.Width)
                BytesPerLine = (_clipRegion.Width - (X-_clipRegion.X)) * 2;

            // Adjust Height
            if (Y + Bitmap.Height > _clipRegion.X + _clipRegion.Width)
                LineCount = _clipRegion.Height - (Y - _clipRegion.Y);

            // Copy Lines
            // Point 1 = (Y * this._w * 2) + (X * 2)
            // Point 2 = (i * Bitmap.Width)
            for (int i = LineStart; i < LineCount + LineStart; i++)
                Array.Copy(Bitmap.Pixels, (i * (Bitmap.Width * 2)) + xOffset, _data, ((Y + i - LineStart) * this._w * 2) + (X * 2), BytesPerLine);
        }

        /// <summary>
        /// Draws another image on top of the current one at the specified location
        /// </summary>
        /// <param name="Bitmap">Raw PicoBitmap to draw</param>
        /// <param name="X">X to draw at</param>
        /// <param name="Y">Y to draw at</param>
        public void DrawImage(byte[] Bitmap, int X, int Y)
        {
            int w = (Bitmap[0] << 8) + Bitmap[1];
            int h = (Bitmap[2] << 8) + Bitmap[3];
            if (Bitmap.Length != w * h * 2 + 4)
                return;

            int BytesPerLine = w * 2;
            int LineCount = h;
            int xOffset = 0;
            int LineStart = 0;

            // Check bounds
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0 || X >= _clipRegion.X + _clipRegion.Width || Y >= _clipRegion.Y + _clipRegion.Height)
                return;

            // Adjust X
            if (X < 0)
            {
                xOffset = -X * 2;
                X = 0;
            }
            if (X < _clipRegion.X)
            {
                xOffset += (_clipRegion.X - X) * 2;
                X = _clipRegion.X;
            }

            // Adjust Y
            if (Y < 0)
            {
                LineStart = -Y;
                Y = 0;
            }
            if (Y < _clipRegion.Y)
            {
                LineStart += _clipRegion.Y - Y;
                Y = _clipRegion.Y;
            }

            // Adjust Width
            if (X + w > _clipRegion.X + _clipRegion.Width)
                BytesPerLine = (_clipRegion.Width - (X - _clipRegion.X)) * 2;

            // Adjust Height
            if (Y + h > _clipRegion.X + _clipRegion.Width)
                LineCount = _clipRegion.Height - (Y - _clipRegion.Y);

            // Copy Lines
            for (int i = LineStart; i < LineCount + LineStart; i++)
                Array.Copy(Bitmap, (i * (w * 2)) + xOffset, _data, ((Y + i - LineStart) * this._w * 2) + (X * 2), BytesPerLine);
        }

        /// <summary>
        /// Draws a line
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X1">Start X</param>
        /// <param name="Y1">Start Y</param>
        /// <param name="X2">End X</param>
        /// <param name="Y2">End Y</param>
        public void DrawLine(RGB565 Color, int X1, int Y1, int X2, int Y2)
        {
            DrawLine(Color.Value, X1, Y1, X2, Y2);
        }

        /// <summary>
        /// Draws a line
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X1"></param>
        /// <param name="Y1"></param>
        /// <param name="X2"></param>
        /// <param name="Y2"></param>
        public void DrawLine(ushort Color, int X1, int Y1, int X2, int Y2)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0)
                return;

            int dy = Y2 - Y1;
            int dx = X2 - X1;

            float m = 0;
            int b = 0;

            if (dx != 0)
            {
                m = ((float)(dy)) / (dx);
                b = Y1 - (int)(m * X1);
            }

            if (global::System.Math.Abs(dx) >= global::System.Math.Abs(dy))
            {
                if (X1 > X2)
                {
                    Swap(ref X1, ref X2);
                    Swap(ref Y1, ref Y2);
                }

                while (X1 <= X2)
                {
                    SetPixel(Color, X1, Y1);

                    X1++;

                    if (X1 <= X2)
                        Y1 = (int)(m * X1) + b;
                }
            }
            else
            {
                if (Y1 > Y2)
                {
                    Swap(ref X1, ref X2);
                    Swap(ref Y1, ref Y2);
                }

                while (Y1 <= Y2)
                {
                    SetPixel(Color, X1, Y1);

                    Y1++;

                    if (Y1 <= Y2 && dx != 0)
                            X1 = (int)((float)(Y1 - b) / m);
                }
            }
        }

        /// <summary>
        /// Draws a Rectangle
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Width"></param>
        /// <param name="Height"></param>
        public void DrawRectangle(RGB565 Color, int X, int Y, int Width, int Height)
        {
            DrawRectangle(Color.Value, X, Y, Width, Height);
        }

        /// <summary>
        /// Draws a Rectangle
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Width"></param>
        /// <param name="Height"></param>
        public void DrawRectangle(ushort Color, int X, int Y, int Width, int Height)
        {
            FillRectangle(Color, X, Y, Width, 1);
            FillRectangle(Color, X, Y, 1, Height);
            FillRectangle(Color, X + Width - 1, Y, 1, Height);
            FillRectangle(Color, X, Y + Height - 1, Width, 1);
        }

        /// <summary>
        /// Draws a Rectangle
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Width"></param>
        /// <param name="Height"></param>
        /// <param name="FillColor"></param>
        public void DrawRectangle(RGB565 Color, int X, int Y, int Width, int Height, RGB565 FillColor)
        {
            DrawRectangle(Color.Value, X, Y, Width, Height, FillColor.Value);
        }

        /// <summary>
        /// Draws a Rectangle
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Width"></param>
        /// <param name="Height"></param>
        /// <param name="FillColor"></param>
        public void DrawRectangle(ushort Color, int X, int Y, int Width, int Height, ushort FillColor)
        {
            FillRectangle(Color, X, Y, Width, 1);
            FillRectangle(Color, X, Y, 1, Height);
            FillRectangle(Color, X + Width - 1, Y, 1, Height);
            FillRectangle(Color, X, Y + Height - 1, Width, 1);

            FillRectangle(FillColor, X + 1, Y + 1, Width - 2, Height - 2);
        }

        /// <summary>
        /// Draws a filled rectangle with a gradient and border
        /// </summary>
        /// <param name="OutlineColor">Color to use for border</param>
        /// <param name="OutlineWidth">Width of border</param>
        /// <param name="X">X location</param>
        /// <param name="Y">Y location</param>
        /// <param name="Width">Width of rectangle</param>
        /// <param name="Height">Height of rectangle</param>
        /// <param name="Gradient1">First color</param>
        /// <param name="Gradient2">Second color</param>
        public void DrawRectangle(RGB565 OutlineColor, int OutlineWidth, int X, int Y, int Width, int Height, RGB565 Gradient1, RGB565 Gradient2, bool Horizontal = false)
        {
            int OW2 = OutlineWidth * 2;

            // Draw Outline
            if (OutlineWidth > 0)
            {
                FillRectangle(OutlineColor.Value, X, Y, Width, OutlineWidth);
                FillRectangle(OutlineColor.Value, X, Y + Height - OutlineWidth, Width, OutlineWidth);
                FillRectangle(OutlineColor.Value, X, Y + OutlineWidth, OutlineWidth, Height - OW2);
                FillRectangle(OutlineColor.Value, X + Width - OutlineWidth, Y + OutlineWidth, OutlineWidth, Height - OW2);
            }

            // Update Vars
            X += OutlineWidth;
            Y += OutlineWidth;
            Width -= OW2;
            Height -= OW2;

            // Check for same values
            if (Gradient1.Value == Gradient2.Value)
            {
                FillRectangle(Gradient1.Value, X, Y, Width, Height);
                return;
            }

            // Get Values
            double R = Gradient1.Red;
            double G = Gradient1.Green;
            double B = Gradient1.Blue;
            double rS;
            double gS;
            double bS;
            RGB565 c;


            if (Horizontal)
            {
                rS = (double)(Gradient2.Red - Gradient1.Red) / Width;
                gS = (double)(Gradient2.Green - Gradient1.Green) / Width;
                bS = (double)(Gradient2.Blue - Gradient1.Blue) / Width;

                for (int i = X; i < X + Width; i++)
                {
                    c = new RGB565((byte)R, (byte)G, (byte)B);
                    FillRectangle(c.Value, i, Y, 1, Height);
                    R += rS;
                    G += gS;
                    B += bS;
                }
            }
            else
            {
                rS = (double)(Gradient2.Red - Gradient1.Red) / Height;
                gS = (double)(Gradient2.Green - Gradient1.Green) / Height;
                bS = (double)(Gradient2.Blue - Gradient1.Blue) / Height;

                for (int i = Y; i < Y + Height; i++)
                {
                    c = new RGB565((byte)R, (byte)G, (byte)B);
                    FillRectangle(c.Value, X, i, Width, 1);
                    R += rS;
                    G += gS;
                    B += bS;
                }
            }

        }

        /// <summary>
        /// Loads a PicoBitmap of the same size directly into current PicoBitmap
        /// </summary>
        /// <param name="PicoBmpData">Raw PicoBitmap data</param>
        public void LoadFile(byte[] PicoBmpData)
        {
            Array.Copy(PicoBmpData, 4, _data, 0, _data.Length);
        }

        /// <summary>
        /// Returns pixel at specific location (RGB565)
        /// </summary>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <returns></returns>
        public RGB565 GetPixel(int X, int Y)
        {
            if (X > _w - 1 || Y > _h - 1)
                throw new Exception("Invalid location");

            int l = (Y * _w * 2) + (X * 2);
            int val = _data[l] << 8;
            val += _data[l + 1];

            return new RGB565((ushort)val);
        }

        /// <summary>
        /// Returns pixel at specific location (RAW)
        /// </summary>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <returns></returns>
        public ushort GetPixelRaw(int X, int Y)
        {
            if (X > _w - 1 || Y > _h - 1)
                throw new Exception("Invalid location");

            int l = (Y * _w * 2) + (X * 2);
            return (ushort)(_data[l] << 8 + _data[l + 1]);
        }

        /// <summary>
        /// Saves a TinyBitmap to file
        /// </summary>
        /// <param name="Filename"></param>
        public void Save(string Filename)
        {
            if (File.Exists(Filename))
                File.Delete(Filename);

            FileStream FS = new FileStream(Filename, FileMode.CreateNew, FileAccess.Write);
            byte[] bOut = new byte[4];

            bOut[0] = (byte)(_w >> 8);
            bOut[1] = (byte)(_w);
            bOut[2] = (byte)(_h >> 8);
            bOut[3] = (byte)(_h);
            FS.Write(bOut, 0, 4);
            FS.Write(_data, 0, _data.Length);
            FS.Close();
            FS.Dispose();
            bOut = null;
        }

        /// <summary>
        /// Sets a pixel value
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        public void SetPixel(RGB565 Color, int X, int Y)
        {
            SetPixel(Color.Value, X, Y);
        }

        /// <summary>
        /// Sets a pixel value
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        public void SetPixel(ushort Color, int X, int Y)
        {
            if (_clipRegion.Width == 0 || _clipRegion.Height == 0 || X < _clipRegion.X || Y < _clipRegion.Y || X > _clipRegion.X + _clipRegion.Width || Y > _clipRegion.Y + _clipRegion.Height)
                return;

            int l = (Y * _w * 2) + (X * 2);
            _data[l] = (byte)(Color >> 8);
            _data[l + 1] = (byte)Color;
        }

        #endregion

        #region Public Methods (PicoFont)

        /// <summary>
        /// Draws a string (transparent background)
        /// </summary>
        /// <param name="Text">Text to draw</param>
        /// <param name="Font">Font to use</param>
        /// <param name="Color">Color to use for font</param>
        /// <param name="X">X location to draw at</param>
        /// <param name="Y">Y location to draw at</param>
        public void DrawText(string Text, PicoFont Font, RGB565 Color, int X, int Y)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0 || Y > _clipRegion.Y + _clipRegion.Height || X > _clipRegion.X + _clipRegion.Width)
                return;

            for (int i = 0; i < Text.Length; i++)
            {
                X += DrawChar_Trans(Text[i], Font, X, Y, Color.Value) + 1;
                if (X >= _w)
                    return;
            }

        }

        /// <summary>
        /// Draws a string (opaque background)
        /// </summary>
        /// <param name="Text">Text to draw</param>
        /// <param name="Font">Font to use</param>
        /// <param name="Color">Color to use for font</param>
        /// <param name="X">X location to draw at</param>
        /// <param name="Y">Y location to draw at</param>
        /// <param name="BackColor">Background color</param>
        public void DrawText(string Text, PicoFont Font, RGB565 Color, int X, int Y, RGB565 BackColor)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0 || Y > _clipRegion.Y + _clipRegion.Height || X > _clipRegion.X + _clipRegion.Width)
                return;

            for (int i = 0; i < Text.Length; i++)
            {
                X += DrawChar_Opaque(Text[i], Font, X, Y, Color.Value, BackColor.Value) + 1;
                if (X >= _w)
                    return;
            }
        }

        /// <summary>
        /// Draws a screen in rect (transparent background)
        /// </summary>
        /// <param name="Text">Text to draw</param>
        /// <param name="Font">Font to use</param>
        /// <param name="Color">Color to use for font</param>
        /// <param name="X">X location to draw at</param>
        /// <param name="Y">Y location to draw at</param>
        /// <param name="Width">Width of region to draw inside</param>
        /// <param name="Height">Height of region to draw inside</param>
        /// <param name="Center">Centers text when true</param>
        public void DrawText(string Text, PicoFont Font, RGB565 Color, int X, int Y, int Width, int Height, bool Center = false)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0 || Y > _clipRegion.Y + _clipRegion.Height || X > _clipRegion.X + _clipRegion.Width)
                return;

            // Check for string that fits
            if (Font.MeasureString(Text).Width <= Width)
            {
                DrawText(Text, Font, Color, X + ((Center) ? (Width / 2 - Font.MeasureString(Text).Width / 2) : 0), Y);
                return;
            }

            Height += Y;

            int iPos;
            string str = string.Empty;
            string tmp = string.Empty;
            int tmpW;
            int curW = 0;
            int spcW = Font.MeasureCharacter(' ').Width;

            // Create string that fits
            while (Text.Length > 0)
            {
                // Get next instance of space
                iPos = Text.IndexOf(' ');

                if (iPos > -1)
                {
                    // We've found a space
                    // Grab the chunk as a temp
                    tmp = Text.Substring(0, iPos + 1);
                }
                else
                {
                    tmp = Text;
                }
                tmpW = Font.MeasureString(tmp).Width - spcW;

                // See if we can manage to append it
                if (curW + tmpW < Width)
                {
                    curW += tmpW;
                    str += tmp;
                    if (iPos == -1)
                    {
                        DrawText(str, Font, Color, X + ((Center) ? (Width / 2 - curW / 2) : 0), Y);
                        Text = string.Empty;
                        str = string.Empty;
                    }
                    else
                        Text = Text.Substring(iPos + 1);
                }
                else
                {
                    // Do we already have string data?
                    if (str != string.Empty)
                    {
                        // Flush what we have
                        DrawText(str, Font, Color, X, Y);

                        // Linefeed
                        Y += Font.FontHeight;
                        if (Y + Font.FontHeight > Height)
                            return;

                        // Can we copy the new temp?
                        if (tmpW < Width)
                        {
                            str = tmp;
                            curW = tmpW;
                            Text = Text.Substring(iPos + 1);
                        }
                        else
                        {
                            // Can't fit, leave for next loop
                            curW = 0;
                            str = string.Empty;
                        }
                    }
                    else
                    {
                        // Grab as much of the string as we can
                        while (Font.MeasureString(tmp).Width > Width)
                        {
                            tmp = tmp.Substring(0, tmp.Length - 1);
                            iPos -= 1;
                        }

                        // Ensure we have at least 1 character
                        if (tmp.Length == 0)
                        {
                            iPos = 1;
                            tmp = Text.Substring(0, 1);
                        }

                        // Paint out
                        DrawText(tmp, Font, Color, X, Y + ((Center) ? (Width / 2 - curW / 2) : 0));

                        // Linefeed
                        Y += Font.FontHeight;
                        if (Y + Font.FontHeight > Height)
                            return;

                        // Update
                        str = string.Empty;
                        curW = 0;
                        Text = Text.Substring(iPos);
                    }
                }

            }

            if (str != string.Empty)
                DrawText(str, Font, Color, X, Y + ((Center) ? (Width / 2 - curW / 2) : 0));

        }

        /// <summary>
        /// Draws a screen in rect (opaque background)
        /// </summary>
        /// <param name="Text">Text to draw</param>
        /// <param name="Font">Font to use</param>
        /// <param name="Color">Color to use for font</param>
        /// <param name="X">X location to draw at</param>
        /// <param name="Y">Y location to draw at</param>
        /// <param name="Width">Width of region to draw inside</param>
        /// <param name="Height">Height of region to draw inside</param>
        /// <param name="BackColor">Background color</param>
        /// <param name="Center">Centers text when true</param>
        public void DrawText(string Text, PicoFont Font, RGB565 Color, int X, int Y, int Width, int Height, RGB565 BackColor, bool Center = false)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0 || Y > _clipRegion.Y + _clipRegion.Height || X > _clipRegion.X + _clipRegion.Width)
                return;

            // Check for string that fits
            if (Font.MeasureString(Text).Width <= Width)
            {
                DrawText(Text, Font, Color, X + ((Center) ? (Width / 2 - Font.MeasureString(Text).Width / 2) : 0), Y, BackColor);
                return;
            }

            Height += Y;

            int iPos;
            string str = string.Empty;
            string tmp = string.Empty;
            int tmpW;
            int curW = 0;
            int spcW = Font.MeasureCharacter(' ').Width;

            // Create string that fits
            while (Text.Length > 0)
            {
                // Get next instance of space
                iPos = Text.IndexOf(' ');

                if (iPos > -1)
                {
                    // We've found a space
                    // Grab the chunk as a temp
                    tmp = Text.Substring(0, iPos + 1);
                }
                else
                {
                    tmp = Text;
                }
                tmpW = Font.MeasureString(tmp).Width - spcW;

                // See if we can manage to append it
                if (curW + tmpW < Width)
                {
                    curW += tmpW;
                    str += tmp;
                    if (iPos == -1)
                    {
                        DrawText(str, Font, Color, X + ((Center) ? (Width / 2 - curW / 2) : 0), Y, BackColor);
                        Text = string.Empty;
                        str = string.Empty;
                    }
                    else
                        Text = Text.Substring(iPos + 1);
                }
                else
                {
                    // Do we already have string data?
                    if (str != string.Empty)
                    {
                        // Flush what we have
                        DrawText(str, Font, Color, X, Y, BackColor);

                        // Linefeed
                        Y += Font.FontHeight;
                        if (Y + Font.FontHeight > Height)
                            return;

                        // Can we copy the new temp?
                        if (tmpW < Width)
                        {
                            str = tmp;
                            curW = tmpW;
                            Text = Text.Substring(iPos + 1);
                        }
                        else
                        {
                            // Can't fit, leave for next loop
                            curW = 0;
                            str = string.Empty;
                        }
                    }
                    else
                    {
                        // Grab as much of the string as we can
                        while (Font.MeasureString(tmp).Width > Width)
                        {
                            tmp = tmp.Substring(0, tmp.Length - 1);
                            iPos -= 1;
                        }

                        // Ensure we have at least 1 character
                        if (tmp.Length == 0)
                        {
                            iPos = 1;
                            tmp = Text.Substring(0, 1);
                        }

                        // Paint out
                        DrawText(tmp, Font, Color, X, Y + ((Center) ? (Width / 2 - curW / 2) : 0), BackColor);

                        // Linefeed
                        Y += Font.FontHeight;
                        if (Y + Font.FontHeight > Height)
                            return;

                        // Update
                        str = string.Empty;
                        curW = 0;
                        Text = Text.Substring(iPos);
                    }
                }

            }

            if (str != string.Empty)
                DrawText(str, Font, Color, X, Y + ((Center) ? (Width / 2 - curW / 2) : 0), BackColor);
        }

        #endregion
        
        #region Private Methods (General)

        /// <summary>
        /// Draws 4 points for DrawEllipse
        /// </summary>
        /// <param name="xCenter"></param>
        /// <param name="yCenter"></param>
        /// <param name="x"></param>
        /// <param name="y"></param>
        /// <param name="Color"></param>
        private void ellipsePlotPoints(int xCenter, int yCenter, int x, int y, ushort Color)
        {
            SetPixel(Color, xCenter + x, yCenter + y);
            SetPixel(Color, xCenter - x, yCenter + y);
            SetPixel(Color, xCenter + x, yCenter - y);
            SetPixel(Color, xCenter - x, yCenter - y);
        }

        /// <summary>
        /// Draws a filled rectangle
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Width"></param>
        /// <param name="Height"></param>
        private void FillRectangle(RGB565 Color, int X, int Y, int Width, int Height)
        {
            FillRectangle(Color.Value, X, Y, Width, Height);
        }

        /// <summary>
        /// Draws a filled rectangle
        /// </summary>
        /// <param name="Color"></param>
        /// <param name="X"></param>
        /// <param name="Y"></param>
        /// <param name="Width"></param>
        /// <param name="Height"></param>
        private void FillRectangle(ushort Color, int X, int Y, int Width, int Height)
        {
            if (_clipRegion.Height == 0 || _clipRegion.Width == 0)
                return;

            // Check bounds
            if (X >= _clipRegion.X + _clipRegion.Width || Y >= _clipRegion.Y + _clipRegion.Height)
                return;

            // Adjust X
            if (X < 0)
            {
                Width += X;
                X = 0;
            }
            if (X < _clipRegion.X)
            {
                Width -= _clipRegion.X - X;
                X = _clipRegion.X;
            }

            // Adjust Y
            if (Y < 0)
            {
                Height += Y;
                Y = 0;
            }
            if (Y < _clipRegion.Y)
            {
                Height -= _clipRegion.Y - Y;
                Y = _clipRegion.Y;
            }

            // Adjust Width
            if (X + Width > _clipRegion.X + _clipRegion.Width)
                Width = (_clipRegion.Width - _clipRegion.X) - X;

            // Adjust Height
            if (Y + Height > _clipRegion.Y + _clipRegion.Height)
                Height = (_clipRegion.Height - _clipRegion.Y) - Y;

            byte c1 = (byte)(Color >> 8);
            byte c2 = (byte)Color;
            int l, i, ii;

            for (i = 0; i < Height; i++)
            {
                l = (Y + i) * _w * 2 + (X * 2);
                for (ii = 0; ii < Width; ii++)
                {
                    _data[l++] = c1;
                    _data[l++] = c2;
                }
            }

        }

        /// <summary>
        /// Get point spacing for elipses
        /// </summary>
        /// <param name="radius"></param>
        /// <returns></returns>
        private float GetPointSpacing(int radius)
        {
            float ls = 1.06814128F;
            if (radius < 0)
                return -ls / radius;
            else if (radius == 0)
                return ls;
            else
                return ls / radius;
        }

        /// <summary>
        /// Used by DrawEllipse
        /// </summary>
        /// <param name="a"></param>
        /// <returns></returns>
        private int Round(double a)
        {
            return (int)(a + 0.5);
        }

        /// <summary>
        /// Swaps the values of 2 variables
        /// </summary>
        /// <param name="a1"></param>
        /// <param name="a2"></param>
        private void Swap(ref int a1, ref int a2)
        {
            int temp = a1;
            a1 = a2;
            a2 = temp;
        }

        #endregion

        #region Private Methods (PicoFont)

        /// <summary>
        /// Draws a character with a transparent background at specified location
        /// </summary>
        /// <param name="Character">Character to draw</param>
        /// <param name="Font">Font to use</param>
        /// <param name="X">X location</param>
        /// <param name="Y">Y location</param>
        /// <param name="Color">Color to use</param>
        /// <returns></returns>
        private int DrawChar_Trans(char Character, PicoFont Font, int X, int Y, ushort Color)
        {
            int w = 0;
            int i;
            int cw;

            if (Character < 33 || Character > 126)
                return ((Font._sizes[23] * ((Font.AntiAliased) ? 2 : 8)) / Font.FontHeight) - 1;

            if (Font.FixedWidth)
            {
            }
            else
            {
                // Get Lookup
                int iIndex = Font._lookups[Character - 33];
                byte[] b = new byte[Font._sizes[Character - 33]];
                Array.Copy(Font._fontData, iIndex, b, 0, b.Length);

                // Calculate Width
                w = (b.Length * ((Font.AntiAliased) ? 2 : 8)) / Font.FontHeight;

                if (Font.AntiAliased)
                {
                }
                else
                {
                    cw = 0;
                    for (i = 0; i < b.Length; i++)
                    {
                        if ((b[i] & 0x80) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x40) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x20) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x10) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x8) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x4) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x2) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x1) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }
                    }
                }

            }


            return w;

        }

        /// <summary>
        /// Draws a character with a transparent background at specified location
        /// </summary>
        /// <param name="Character">Character to draw</param>
        /// <param name="Font">Font to use</param>
        /// <param name="X">X location</param>
        /// <param name="Y">Y location</param>
        /// <param name="Color">Color to use</param>
        /// <param name="BkgColor">Background color to use</param>
        /// <returns></returns>
        private int DrawChar_Opaque(char Character, PicoFont Font, int X, int Y, ushort Color, ushort BkgColor)
        {
            int w = 0;
            int i;
            int cw;

            if (Character < 33 || Character > 126)
                return ((Font._sizes[23] * ((Font.AntiAliased) ? 2 : 8)) / Font.FontHeight) - 1;

            if (Font.FixedWidth)
            {
            }
            else
            {
                // Get Lookup
                int iIndex = Font._lookups[Character - 33];
                byte[] b = new byte[Font._sizes[Character - 33]];
                Array.Copy(Font._fontData, iIndex, b, 0, b.Length);

                // Calculate Width
                w = (b.Length * ((Font.AntiAliased) ? 2 : 8)) / Font.FontHeight;

                // Fill background
                FillRectangle(BkgColor, X, Y, w, Font.FontHeight);

                if (Font.AntiAliased)
                {
                }
                else
                {
                    cw = 0;
                    for (i = 0; i < b.Length; i++)
                    {
                        if ((b[i] & 0x80) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x40) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x20) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x10) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x8) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x4) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x2) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }

                        if ((b[i] & 0x1) != 0)
                            SetPixel(Color, X, Y);
                        X += 1;
                        cw += 1;
                        if (cw == w)
                        {
                            X -= cw;
                            cw = 0;
                            Y += 1;
                        }
                    }

                }

            }


            return w;

        }

        #endregion

    }
}
